/*
准备放在树莓派上，输入更改为视频流rtsp
两个画面 需要配置保存和展示
*/


#include <iostream>
#include "func.h"
const char* params
    = "{ help h         |           | Print usage }"
      "{ lr             | -1        | learning rate of MOG2 }"
	  "{ show           |           | show or not  }"
      "{ algo           | KNN       | Background subtraction method (KNN, MOG2) }"
	  "{ output         |           | output path }";
	  
int main(int argc, char* argv[])
{
    cv::CommandLineParser parser(argc, argv, params);
    parser.about( "This program shows how to use background subtraction methods provided by "
                  " OpenCV. You can process both videos and images.\n" );
    if (parser.has("help"))
    {
        //print help information
        parser.printMessage();
		return 0;    
	}
    //create Background Subtractor objects
	    cv::Ptr<cv::BackgroundSubtractor> pBackSub;
    	if (parser.get<cv::String>("algo") == "MOG2")
        	pBackSub = cv::createBackgroundSubtractorMOG2(500,16,true);
    	else
        	pBackSub = cv::createBackgroundSubtractorKNN(500,400,true);
        
		std::string rtsp = "rtsp://admin:i999999999@169.254.123.128:554";
		cv::VideoWriter videoWriter;
		cv::VideoCapture capture(rtsp);
		if (!capture.isOpened()){
            //error in opening the video input
            return 0;
        }
	    int width =(int)capture.get(cv::CAP_PROP_FRAME_WIDTH);
	    int height = (int)capture.get(cv::CAP_PROP_FRAME_HEIGHT);
        cv::Size S = cv::Size(2*(int)capture.get(cv::CAP_PROP_FRAME_WIDTH),(int)capture.get(cv::CAP_PROP_FRAME_HEIGHT));
        int fps = capture.get(cv::CAP_PROP_FPS);
		cv::Mat raw(width, height, CV_8UC3),frame, fgMask;
		capture.release();
		std::thread thread(cameraThreadFunc,  std::ref(capture),std::ref(raw));
		
		CArea* area=new CArea[410];//把区域分成10*10的方格
		bool flag = 1;
		if (parser.has("output")){
            //videoWriter.open("./OUT_"+parser.get<cv::String>("output")+".mp4", cv::CAP_OPENCV_MJPEG,fps/2 ,S);
	    //videoWriter.open("./OUT_"+parser.get<cv::String>("output")+".avi", cv::VideoWriter::fourcc('X','V','I','D'),fps/2 ,S);
	    videoWriter.open("./OUT_"+parser.get<cv::String>("output")+".avi", cv::VideoWriter::fourcc('M','J','P','G'),fps/2 ,S);
        }

        while (true) {
			mutex.lock();
			raw.copyTo(frame);
        mutex.unlock();
            //update the background model
       	    if (parser.get<cv::String>("algo") == "MOG2")
                pBackSub->apply(frame, fgMask,parser.get<double>("lr"));
            else
                pBackSub->apply(frame, fgMask);
			
			cv::Mat resultImg =cv::Mat(height ,2*width,CV_8UC3, cv::Scalar::all(0));
            cv::Mat ROI_1 = resultImg(cv::Rect(0, 0, width, height));
            cv::Mat ROI_2 = resultImg(cv::Rect(width, 0, width, height));
			//frame.copyTo(ROI_1);
			cv::Mat fgMask3Channels = convertTo3Channels(fgMask);
			std::vector<std::vector<cv::Point>> contours;
			
            //开操作 先腐蚀再膨胀
			cv::Mat se2 = cv::getStructuringElement(0,cv::Size(2,2) ); //构造矩形结构元素
			erode(fgMask, fgMask, se2, cv::Point(-1, -1), 1); //执行腐蚀操作
            dilate(fgMask, fgMask, se2, cv::Point(-1, -1), 1); //执行膨胀操作
			//闭操作 先膨胀再腐蚀
			cv::Mat se3 = cv::getStructuringElement(0,cv::Size(3,3) ); //构造矩形结构元素
			dilate(fgMask, fgMask, se3, cv::Point(-1, -1), 1); //执行膨胀操作
            erode(fgMask, fgMask, se3, cv::Point(-1, -1), 1); //执行腐蚀操作

			fgMask3Channels = convertTo3Channels(fgMask);
			findContours(fgMask, contours, cv::RETR_EXTERNAL,cv::CHAIN_APPROX_SIMPLE);	
			CArea* new_area = new CArea[410];//每次都更新就不需要时间戳了
			int count = 0;
         	for (int i = 0; i < contours.size(); ++i)
        	{
				//判断轮廓大小，当大于30时进入
            	if(cv::contourArea(contours[i])>30)
            	{
					count++;
					//获取轮廓矩形
					cv::Rect rect=cv::boundingRect(contours[i]);
						
					//加入区域中
					double x = rect.x;
					double y = rect.y;
					int posx = int(20.0*rect.x/width);
					int posy = int(20.0*rect.y/height);
					int pos_in_area =posx * 20 + posy;
					//输出一下位置看看？

					//cv::putText(fgMask3Channels,std::to_string(pos_in_area),
					//	cv::Point(rect.x,rect.y+12), cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(255, 255, 255));
					new_area[pos_in_area].add_contour(CMyContour(contours[i],x,y,count));	
					double min_match = 30;
					int match_num=0;
					for(int ii = posx - 1; ii <= posx + 1; ++ii){
						if(ii < 0 || ii >= 20)continue;
						for(int jj = posy - 1; jj < posy + 1; ++jj){
							if(jj<0||jj>=20)continue;
								for(std::vector<CMyContour>::iterator it = area[ii*20+jj].myContours.begin(); it != area[ii*20+jj].myContours.end();it++){
									if( cv::matchShapes(contours[i],(*it).contour,2,0.0) < min_match
										&&calc_dis(x,y,(*it).x,(*it).y)/sqrt(width*width+height*height)<0.03){ //匹配成功 为什么这个值这么大都可以匹配成功呢
										min_match = cv::matchShapes(contours[i],(*it).contour,2,0.0);
										match_num = (*it).num;
										}
									}	
							}
						}
					if(min_match<30){	
						//所有涂色都在三通道图像上进行
						cv::putText(fgMask3Channels,std::to_string(count)+
							"match"+std::to_string(match_num)+":"+std::to_string(min_match), cv::Point(rect.x,rect.y),
							cv::FONT_HERSHEY_PLAIN,1, cv::Scalar(255, 255, 255));
						cv::drawContours(fgMask3Channels,contours,i,cv::Scalar(127,255,0),-1);
						}
					else{
						//在矩形的左上角点输出文字 这次的序号
						cv::putText(fgMask3Channels, "new "+std::to_string(count), cv::Point(rect.x,rect.y),
							cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(255, 255, 255));
						//绘制轮廓  新出现的是红色
						cv::drawContours(fgMask3Channels,contours,i,cv::Scalar(249,72,247),-1);
						}
				}
        	}
			//用完清空
			delete []area;
			area = new_area;
			fgMask3Channels.copyTo(ROI_2);
			//frame.copyTo(ROI_2);
			if (parser.has("output")){
                videoWriter.write(resultImg);
            }
            if (parser.has("show")){
                cv::imshow("image",resultImg);
            }

			//get the input from the keyboard
            int keyboard = cv::waitKey(30);
            if (keyboard == 'q' || keyboard == 27)
                break;
        }
		delete []area;
		if (parser.has("output")){
            videoWriter.release();
        }
	isOpen=false;
	cv::destroyAllWindows();
		thread.join();
        return 0;
}
